#include "ArriveBehaviour.h"
#include "BehaviourInfo.h"
#include "BPhysicsActor.h"
#include "InteractiveState.h"

void ArriveBehaviour::CalcMotion(float* motion, BehaviourInfo* info, float weighting)
{
    if(!info->GetTargetActor() && !info->GetTargetPosition())
        return;
    //get the direction we want to be heading in
    btVector3 force;
    if(info->GetTargetActor())
        force = info->GetTargetActor()->GetPosition();
    else
        force = *info->GetTargetPosition();
    force -= info->GetSourceActor()->GetPosition();
    btScalar dist = force.length();
    dist *= info->GetArriveParameter();
    //scale it by the maximum velocity
    force.normalize();
    force *= min(info->GetMaxVelocity(),dist);

    //now minus the current velocity from it,
    force -= info->GetSourceActor()->GetLinearVelocity();
   // Engine::Instance()->drawLine(info->GetSourceActor()->GetPosition(),
                               //info->GetSourceActor()->GetPosition() + force, btVector3(0,1,0));
    BehaviourI::FillArray(motion, &force, weighting);
}